
#include"kalmanKF.h"
MykalmanKF::MykalmanKF()
{
    stateNum = 4;
    measureNum = 2;
    measurement = Mat::zeros(measureNum, 1, CV_32F);
}
void MykalmanKF::kalmanInit()
{
    KF.init(stateNum,measureNum); 
    KF.transitionMatrix = (Mat_<float>(4, 4) <<1,0,40,0,
                                                                                        0,1,0,40,
                                                                                        0,0,1,0,
                                                                                        0,0,0,1);  //转移矩阵A
	setIdentity(KF.measurementMatrix);                                             //测量矩阵H
	setIdentity(KF.processNoiseCov, Scalar::all(1e-5));                            //系统噪声方差矩阵Q
	setIdentity(KF.measurementNoiseCov, Scalar::all(1e-1));                        //测量噪声方差矩阵R
	setIdentity(KF.errorCovPost, Scalar::all(1));                                  //后验错误估计协方差矩阵P
    KF.statePost = (Mat_<float>(4, 1) << 0, 0, 0, 0);//Ensure beginner is default value
}
Point2f MykalmanKF::kalmanPredict(float x ,float y)
{
     //2.kalman prediction
	Mat prediction = KF.predict();
	Point predict_pt = Point(prediction.at<float>(0),prediction.at<float>(1) );   //预测值(x',y')
 
	//3.update measurement
	measurement.at<float>(0) = x;
	measurement.at<float>(1) = y;		
 
	//4.update
	KF.correct(measurement);
    return predict_pt;
}
